#include "freertos/FreeRTOS.h"
#include "driver/gpio.h"
#include "driver/ledc.h"
#include "esp_log.h"

#include "motor.h"
#include "main.h"

#define TAG "MOTOR"

motor_t motor1;
motor_t motor2;
motor_t motor3;

TaskHandle_t motor_control_task_handle;

/**
 * @brief 初始化振动马达
 * @param motor 马达配置结构体
 * @return ESP_OK成功，其他值失败
 */
esp_err_t hal_motor_init(motor_t *motor)
{
    esp_err_t err;
    // 配置LEDC定时器，频率1kHz
    ledc_timer_config_t ledc_timer = {
        .speed_mode      = LEDC_LOW_SPEED_MODE,
        .timer_num       = motor->ledc_timer,
        .duty_resolution = LEDC_TIMER_9_BIT,
        .freq_hz         = 1000, // 1kHz PWM频率
        .clk_cfg         = LEDC_AUTO_CLK};
    err = ledc_timer_config(&ledc_timer);
    if (err != ESP_OK) {
        ESP_LOGE(TAG, "failed to config ledc timer");
        return err;
    }

    // 配置LEDC通道
    ledc_channel_config_t ledc_channel = {
        .speed_mode = LEDC_LOW_SPEED_MODE,
        .channel    = motor->ledc_channel,
        .timer_sel  = motor->ledc_timer,
        .intr_type  = LEDC_INTR_DISABLE,
        .gpio_num   = motor->motor_gpio,
        .duty       = 0, // 初始占空比为0
        .hpoint     = 0};
    err = ledc_channel_config(&ledc_channel);
    if (err != ESP_OK) {
        ESP_LOGE(TAG, "failed to config ledc channel");
        return err;
    }

    return ESP_OK;
}

/**
 * @brief 开启振动马达
 * @param motor 马达配置结构体
 * @return ESP_OK成功，其他值失败
 */
esp_err_t hal_motor_on(motor_t *motor)
{
    esp_err_t err;
    // 根据配置的占空比设置PWM
    err = ledc_set_duty(LEDC_LOW_SPEED_MODE, motor->ledc_channel, motor->duty_rate * 512);
    if (err != ESP_OK) {
        ESP_LOGE(TAG, "failed to set duty");
        return err;
    }

    err = ledc_update_duty(LEDC_LOW_SPEED_MODE, motor->ledc_channel);
    if (err != ESP_OK) {
        ESP_LOGE(TAG, "failed to update duty");
        return err;
    }
    
    motor->enable_flag = true;

    return ESP_OK;
}

/**
 * @brief 关闭振动马达
 * @param motor 马达配置结构体
 * @return ESP_OK成功，其他值失败
 */
esp_err_t hal_motor_off(motor_t *motor)
{
    esp_err_t err;
    // 设置0%占空比
    err = ledc_set_duty(LEDC_LOW_SPEED_MODE, motor->ledc_channel, 0);
    if (err != ESP_OK) {
        ESP_LOGE(TAG, "failed to set duty");
        return err;
    }

    err = ledc_update_duty(LEDC_LOW_SPEED_MODE, motor->ledc_channel);
    if (err != ESP_OK) {
        ESP_LOGE(TAG, "failed to update duty");
        return err;
    }
    
    motor->enable_flag = false;

    return ESP_OK;
}

/**
 * @brief 设置振动马达占空比
 * @param motor 马达配置结构体
 * @param duty_rate 占空比
 * @return ESP_OK成功，其他值失败
 */
esp_err_t hal_motor_duty_rate_set(motor_t *motor, float duty_rate)
{
    if (duty_rate < 0)
        duty_rate = 0;
    else if (duty_rate > 1)
        duty_rate = 1;

    motor->duty_rate = duty_rate;

    return ESP_OK;
}

void motor_control_task(void *arg)
{
    uint32_t notify_value;
    while (1) {
        if (xTaskNotifyWait(0, ULONG_MAX, &notify_value, portMAX_DELAY) == pdPASS) {
            switch (notify_value) {
            case MOTOR_ACTON_POS:
                // 向左移动：马达1→马达2→马达3依次振动，模拟从左到右的触觉反馈
                hal_motor_on(&motor1);
                vTaskDelay(500 / portTICK_PERIOD_MS);
                hal_motor_off(&motor1);
                hal_motor_on(&motor2);
                vTaskDelay(500 / portTICK_PERIOD_MS);
                hal_motor_off(&motor2);
                hal_motor_on(&motor3);
                vTaskDelay(500 / portTICK_PERIOD_MS);
                hal_motor_off(&motor3);
                break;
            case MOTOR_ACTON_NEG:
                // 向右移动：马达3→马达2→马达1依次振动，模拟从右到左的触觉反馈
                hal_motor_on(&motor3);
                vTaskDelay(500 / portTICK_PERIOD_MS);
                hal_motor_off(&motor3);
                hal_motor_on(&motor2);
                vTaskDelay(500 / portTICK_PERIOD_MS);
                hal_motor_off(&motor2);
                hal_motor_on(&motor1);
                vTaskDelay(500 / portTICK_PERIOD_MS);
                hal_motor_off(&motor1);
                break;
            case MOTOR_ACTON_BOTH:
                // 翻转动作：三个马达同时振动1秒
                hal_motor_on(&motor1);
                hal_motor_on(&motor2);
                hal_motor_on(&motor3);
                vTaskDelay(1000 / portTICK_PERIOD_MS);
                hal_motor_off(&motor1);
                hal_motor_off(&motor2);
                hal_motor_off(&motor3);
                break;
            }
        }
    }
}

esp_err_t app_motor_init(void)
{
    // 初始化三个振动马达
    // 左下角振动马达
    motor1.motor_gpio = MOTOR1_GPIO;
    motor1.ledc_timer = MOTOR1_LEDC_TIMER;
    motor1.ledc_channel = MOTOR1_LEDC_CHANNEL;
    motor1.duty_rate = MOTOR1_LEDC_DUTY_RATE;
    ESP_ERROR_CHECK(hal_motor_init(&motor1));

    // 中下角振动马达
    motor2.motor_gpio = MOTOR2_GPIO;
    motor2.ledc_timer = MOTOR2_LEDC_TIMER;
    motor2.ledc_channel = MOTOR2_LEDC_CHANNEL;
    motor2.duty_rate = MOTOR2_LEDC_DUTY_RATE;
    ESP_ERROR_CHECK(hal_motor_init(&motor2));

    // 右下角振动马达
    motor3.motor_gpio = MOTOR3_GPIO;
    motor3.ledc_timer = MOTOR3_LEDC_TIMER;
    motor3.ledc_channel = MOTOR3_LEDC_CHANNEL;
    motor3.duty_rate = MOTOR3_LEDC_DUTY_RATE;
    ESP_ERROR_CHECK(hal_motor_init(&motor3));

    xTaskCreate(motor_control_task, "moto_ctrl", 1024 * 2, NULL, 5, &motor_control_task_handle);

    return ESP_OK;
}